#!/usr/bin/env python3
import rospy
import time
import numpy as np
import json
import threading
import tf.transformations as tft
from std_msgs.msg import Float64
from gazebo_msgs.srv import GetModelState, SetModelState
from gazebo_msgs.msg import ModelState
from std_srvs.srv import Empty, EmptyResponse
import paho.mqtt.client as mqtt


class SlidingDoorController:
    def __init__(self):
        # 设备信息 - 更新为闸机控制器
        self.vin = "ROBOT-SIM"  # 可以根据需要修改为ZJZK-SIM
        self.terminal_id = 3    # 闸机主控
        self.frame_id = 0
        
        # 初始化状态标记
        self.dt_state_initialized = False
        self.dt_men_state_initialized = False
        self.initial_close_door_executed = False

        rospy.loginfo("Door control server ready.")

        # MQTT配置
        self.mqtt_client = mqtt.Client()
        self.mqtt_client.on_connect = self.on_mqtt_connect
        self.mqtt_client.on_message = self.on_mqtt_message
        
        # 从ROS参数获取MQTT配置
        self.mqtt_host = rospy.get_param('~mqtt_host', 'localhost')
        self.mqtt_port = rospy.get_param('~mqtt_port', 1883)
        self.mqtt_username = rospy.get_param('~mqtt_username', '')
        self.mqtt_password = rospy.get_param('~mqtt_password', '')
        
        # 设置MQTT认证
        if self.mqtt_username:
            self.mqtt_client.username_pw_set(self.mqtt_username, self.mqtt_password)
        
        # 连接MQTT代理
        try:
            self.mqtt_client.connect(self.mqtt_host, self.mqtt_port, 60)
            self.mqtt_client.loop_start()
        except Exception as e:
            rospy.logerr(f"MQTT连接失败: {e}")

        rospy.loginfo("SlidingDoorController 已启动，提供电梯门和闸机门控制服务，支持ROS服务和MQTT控制")
        
        # 检查初始状态并执行关门操作
        self.check_initial_states()

    def check_initial_states(self):
        """检查dt和dt_men的初始状态，如果都存在则执行关门操作"""
        try:
            # 检查dt模型状态
            dt_state = self.get_model_state("dt", "world")
            if dt_state.success:
                self.dt_state_initialized = True
                rospy.loginfo("dt模型状态获取成功")
            else:
                rospy.logwarn("dt模型状态获取失败，将在稍后重试")
            
            # 检查dt_men模型状态
            dt_men_state = self.get_model_state("dt_men", "world")
            if dt_men_state.success:
                self.dt_men_state_initialized = True
                rospy.loginfo("dt_men模型状态获取成功")
            else:
                rospy.logwarn("dt_men模型状态获取失败，将在稍后重试")
            
            # 如果两个模型状态都获取成功且还没有执行过初始关门操作
            if (self.dt_state_initialized and 
                self.dt_men_state_initialized and 
                not self.initial_close_door_executed):
                
                rospy.loginfo("dt和dt_men模型状态都已获取，执行初始关门操作...")
                self.initial_close_door_executed = True
                
                # 在单独线程中执行关门操作，避免阻塞主线程
                threading.Thread(target=self._execute_initial_close_door).start()
            
            # 如果状态未完全初始化，启动定时器重试
            elif not (self.dt_state_initialized and self.dt_men_state_initialized):
                rospy.loginfo("模型状态未完全初始化，将在2秒后重试...")
                rospy.Timer(rospy.Duration(2.0), self._retry_initial_states, oneshot=True)
                
        except Exception as e:
            rospy.logerr(f"检查初始状态时发生错误: {e}")
            # 发生错误时也启动重试定时器
            rospy.Timer(rospy.Duration(2.0), self._retry_initial_states, oneshot=True)

    def _retry_initial_states(self, event):
        """重试检查初始状态的定时器回调"""
        if not self.initial_close_door_executed:
            rospy.loginfo("重试检查模型初始状态...")
            self.check_initial_states()

    def _execute_initial_close_door(self):
        """执行初始关门操作"""
        try:
            rospy.loginfo("开始执行初始关门操作...")
            self.move_door_smooth(start_offset=0.75, end_offset=0.0)
            rospy.loginfo("初始关门操作完成")
        except Exception as e:
            rospy.logerr(f"初始关门操作失败: {e}")

    def move_door_smooth(self, start_offset, end_offset, steps=20, duration=1.0):
        """
        平滑移动门
        start_offset: 起始偏移量 (m)
        end_offset:   结束偏移量 (m)
        steps:        分段数
        duration:     总时长 (秒)
        """
        dt_state = self.get_model_state("dt", "world")
        if not dt_state.success:
            rospy.logerr("模型 dt 不存在！")
            return

        sleep_time = duration / steps

        # 四元数 -> 旋转矩阵
        q = dt_state.pose.orientation
        quat = [q.x, q.y, q.z, q.w]
        rot_mat = tft.quaternion_matrix(quat)[0:3, 0:3]

        # dt 的世界位置
        dt_pos = np.array([
            dt_state.pose.position.x,
            dt_state.pose.position.y,
            dt_state.pose.position.z
        ])

        for i in range(steps + 1):
            offset = start_offset + (end_offset - start_offset) * i / steps

            # 门在 dt 局部坐标系的位置 (门口中心 -0.29, -0.46, +0.5)
            local_pos = np.array([-0.29 + offset, -0.46, 0.5])

            # 转换到世界坐标
            world_pos = rot_mat.dot(local_pos) + dt_pos

            door_state = ModelState()
            door_state.model_name = "dt_men"
            door_state.pose.position.x = world_pos[0]
            door_state.pose.position.y = world_pos[1]
            door_state.pose.position.z = world_pos[2]

            # 保持和 dt 相同的朝向
            door_state.pose.orientation = dt_state.pose.orientation

            self.set_model_state(door_state)
            time.sleep(sleep_time)

    def on_mqtt_connect(self, client, userdata, flags, rc):
        """MQTT连接回调"""
        if rc == 0:
            rospy.loginfo("MQTT连接成功")
            # 订阅电梯任务主题和闸机任务主题
            client.subscribe("dt/task_manager")
            # client.subscribe("zj/task_manager")
            rospy.loginfo("已订阅主题: dt/task_manager, zj/task_manager")
        else:
            rospy.logerr(f"MQTT连接失败，错误码: {rc}")

    def on_mqtt_message(self, client, userdata, msg):
        """MQTT消息回调"""
        try:
            payload = json.loads(msg.payload.decode())
            rospy.loginfo(f"收到MQTT消息: {payload}")
            
            # 检查消息格式
            if not self.validate_message(payload):
                return
            
            if payload.get("VIN") == "ROBOT-SIM":
                return
            
            # 处理电梯控制指令 (sys_data_type = 1006)
            if payload.get("sys_data_type") == 1006:
                self.handle_elevator_control(payload)
            
            # 处理闸机控制指令 (sys_data_type = 1005)
            elif payload.get("sys_data_type") == 1005:
                self.handle_gate_control(payload)
                
        except json.JSONDecodeError as e:
            rospy.logerr(f"JSON解析错误: {e}")
        except Exception as e:
            rospy.logerr(f"处理MQTT消息时发生错误: {e}")

    def validate_message(self, payload):
        """验证消息格式"""
        required_fields = ["VIN", "terminal_id", "frame_id", "sys_data_type", "time_stamp", "code", "data"]
        for field in required_fields:
            if field not in payload:
                rospy.logerr(f"消息缺少必要字段: {field}")
                return False
        return True

    def handle_elevator_control(self, payload):
        """处理电梯控制指令"""
        try:
            data = payload.get("data", {})
            task_type = data.get("tasktype")
            task_id = data.get("task_id")
            
            response_data = {
                "task_status": 1,  # 1=接收任务
                "error_code": 0,
                "task_id": task_id
            }
            
            if task_type == 1:  # 开门
                rospy.loginfo(f"收到开门指令，任务ID: {task_id}")
                # 在单独线程中执行开门操作，避免阻塞MQTT
                threading.Thread(target=self._execute_open_door, args=(task_id,)).start()
                
            elif task_type == 2:  # 关门
                rospy.loginfo(f"收到关门指令，任务ID: {task_id}")
                # 在单独线程中执行关门操作，避免阻塞MQTT
                threading.Thread(target=self._execute_close_door, args=(task_id,)).start()
            elif task_type == 3:  # 呼叫电梯
                threading.Thread(target=self._call_elecator, args=(task_id,data.get("current_floor"))).start()
            elif task_type == 4:  # 按电梯
                threading.Thread(target=self._press_elecator, args=(task_id,data.get("target_floor"))).start()
            else:
                rospy.logwarn(f"未知的任务类型: {task_type}")
                response_data["task_status"] = 3  # 任务失败
                response_data["error_code"] = 1
            
            # 发送任务接收确认
            self.send_mqtt_response(payload, response_data)
            
        except Exception as e:
            rospy.logerr(f"处理电梯控制指令时发生错误: {e}")
            response_data = {
                "task_status": 3,  # 任务失败
                "error_code": 1,
                "task_id": data.get("task_id", 0)
            }
            self.send_mqtt_response(payload, response_data)

    def handle_gate_control(self, payload):
        """处理闸机控制指令"""
        try:
            data = payload.get("data", {})
            task_type = data.get("tasktype")
            task_id = data.get("task_id")
            
            response_data = {
                "task_status": 1,  # 1=接收任务
                "error_code": 0,
                "task_id": task_id
            }
            
            if task_type == 1:  # 开闸机门
                rospy.loginfo(f"收到开闸机门指令，任务ID: {task_id}")
                # 在单独线程中执行开门操作，避免阻塞MQTT
                threading.Thread(target=self._execute_open_gate, args=(task_id,)).start()
                
            elif task_type == 2:  # 关闸机门
                rospy.loginfo(f"收到关闸机门指令，任务ID: {task_id}")
                # 在单独线程中执行关门操作，避免阻塞MQTT
                threading.Thread(target=self._execute_close_gate, args=(task_id,)).start()
                
            else:
                rospy.logwarn(f"未知的闸机任务类型: {task_type}")
                response_data["task_status"] = 3  # 任务失败
                response_data["error_code"] = 1
            
            # 发送任务接收确认
            self.send_mqtt_response(payload, response_data)
            
        except Exception as e:
            rospy.logerr(f"处理闸机控制指令时发生错误: {e}")
            response_data = {
                "task_status": 3,  # 任务失败
                "error_code": 1,
                "task_id": data.get("task_id", 0)
            }
            self.send_mqtt_response(payload, response_data)

    def _execute_open_gate(self, task_id):
        """执行开闸机门操作"""
        try:
            rospy.loginfo("正在开闸机门...")
            # 调用现有的handle_door_control方法，mode=1表示开门
            fake_req = type('obj', (object,), {'mode': 1})()
            result = self.handle_door_control(fake_req)
            
            if result.success:
                rospy.loginfo("闸机门已打开")
                # 发送任务成功响应
                response_data = {
                    "task_status": 2,  # 任务成功
                    "error_code": 0,
                    "task_id": task_id
                }
            else:
                rospy.logerr("闸机门打开失败")
                response_data = {
                    "task_status": 3,  # 任务失败
                    "error_code": 1,
                    "task_id": task_id
                }
                
            self.send_gate_completion_response(task_id, response_data)
            time.sleep(2)
        except Exception as e:
            rospy.logerr(f"开闸机门操作失败: {e}")
            response_data = {
                "task_status": 3,  # 任务失败
                "error_code": 1,
                "task_id": task_id
            }
            self.send_gate_completion_response(task_id, response_data)

    def _execute_close_gate(self, task_id):
        """执行关闸机门操作"""
        try:
            rospy.loginfo("正在关闸机门...")
            # 调用现有的handle_door_control方法，mode=0表示关门
            fake_req = type('obj', (object,), {'mode': 0})()
            result = self.handle_door_control(fake_req)
            
            if result.success:
                rospy.loginfo("闸机门已关闭")
                # 发送任务成功响应
                response_data = {
                    "task_status": 2,  # 任务成功
                    "error_code": 0,
                    "task_id": task_id
                }
            else:
                rospy.logerr("闸机门关闭失败")
                response_data = {
                    "task_status": 3,  # 任务失败
                    "error_code": 1,
                    "task_id": task_id
                }
            time.sleep(2)
            self.send_gate_completion_response(task_id, response_data)
            
        except Exception as e:
            rospy.logerr(f"关闸机门操作失败: {e}")
            response_data = {
                "task_status": 3,  # 任务失败
                "error_code": 1,
                "task_id": task_id
            }
            self.send_gate_completion_response(task_id, response_data)

    def send_gate_completion_response(self, task_id, response_data):
        """发送闸机任务完成响应"""
        self.frame_id += 1
        response = {
            "VIN": self.vin,
            "terminal_id": self.terminal_id,
            "frame_id": self.frame_id,
            "sys_data_type": 1005,  # 闸机控制指令
            "time_stamp": int(time.time() * 1000),
            "code": 0,  # 响应成功
            "data": response_data
        }
        
        try:
            topic = "dt/task_manager"
            self.mqtt_client.publish(topic, json.dumps(response))
            rospy.loginfo(f"已发送闸机任务完成响应: {response}")
        except Exception as e:
            rospy.logerr(f"发送闸机任务完成响应失败: {e}")

    def _execute_open_door(self, task_id):
        """执行开门操作"""
        try:
            rospy.loginfo("正在开门...")
            self.move_door_smooth(start_offset=0.0, end_offset=0.75)
            rospy.loginfo("门已打开")
            
            # 发送任务成功响应
            response_data = {
                "task_status": 2,  # 任务成功
                "error_code": 0,
                "task_id": task_id
            }
            self.send_task_completion_response(task_id, response_data)
            
        except Exception as e:
            rospy.logerr(f"开门操作失败: {e}")
            response_data = {
                "task_status": 3,  # 任务失败
                "error_code": 1,
                "task_id": task_id
            }
            self.send_task_completion_response(task_id, response_data)

    def _execute_close_door(self, task_id):
        """执行关门操作"""
        try:
            rospy.loginfo("正在关门...")
            self.move_door_smooth(start_offset=0.75, end_offset=0.0)
            rospy.loginfo("门已关闭")
            
            # 发送任务成功响应
            response_data = {
                "task_status": 2,  # 任务成功
                "error_code": 0,
                "task_id": task_id
            }
            self.send_task_completion_response(task_id, response_data)
            
        except Exception as e:
            rospy.logerr(f"关门操作失败: {e}")
            response_data = {
                "task_status": 3,  # 任务失败
                "error_code": 1,
                "task_id": task_id
            }
            self.send_task_completion_response(task_id, response_data)

    def _call_elecator(self, task_id, floor):
        """呼叫电梯"""
        try:
            # 呼叫电梯 延时2s
            print(f"呼叫电梯 到{floor}层 ")
            time.sleep(2)
            # 发送任务成功响应
            response_data = {
                "task_status": 2,  # 任务成功
                "error_code": 0,
                "task_id": task_id
            }
            self.send_task_completion_response(task_id, response_data)
        except Exception as e:
            rospy.logerr(f"呼叫电梯操作失败: {e}")
            response_data = {
                "task_status": 3,  # 任务失败
                "error_code": 1,
                "task_id": task_id
            }
            self.send_task_completion_response(task_id, response_data)

    def _press_elecator(self, task_id, floor):
        """按电梯"""
        try:
            # 按电梯 延时2s
            print(f"按电梯 到{floor}层 ")
            time.sleep(2)
            # 发送任务成功响应
            response_data = {
                "task_status": 2,  # 任务成功
                "error_code": 0,
                "task_id": task_id
            }
            self.send_task_completion_response(task_id, response_data)
        except Exception as e:
            rospy.logerr(f"按电梯操作失败: {e}")
            response_data = {
                "task_status": 3,  # 任务失败
                "error_code": 1,
                "task_id": task_id
            }
            self.send_task_completion_response(task_id, response_data)

    def send_mqtt_response(self, original_payload, response_data):
        """发送MQTT响应"""
        self.frame_id += 1
        response = {
            "VIN": self.vin,
            "terminal_id": self.terminal_id,
            "frame_id": self.frame_id,
            "sys_data_type": original_payload.get("sys_data_type"),
            "time_stamp": int(time.time() * 1000),
            "code": 0,  # 响应成功
            "data": response_data
        }
        
        try:
            # 根据消息类型选择正确的主题
            sys_data_type = original_payload.get("sys_data_type")
            if sys_data_type == 1005:  # 闸机控制指令
                topic = "zj/task_manager"
            elif sys_data_type == 1006:  # 电梯控制指令
                topic = "dt/task_manager"
            else:
                topic = "dt/task_manager"  # 默认主题
                
            self.mqtt_client.publish(topic, json.dumps(response))
            rospy.loginfo(f"已发送MQTT响应到主题 {topic}: {response}")
        except Exception as e:
            rospy.logerr(f"发送MQTT响应失败: {e}")

    def send_task_completion_response(self, task_id, response_data):
        """发送任务完成响应"""
        self.frame_id += 1
        response = {
            "VIN": self.vin,
            "terminal_id": self.terminal_id,
            "frame_id": self.frame_id,
            "sys_data_type": 1006,  # 电梯控制指令
            "time_stamp": int(time.time() * 1000),
            "code": 0,  # 响应成功
            "data": response_data
        }
        
        try:
            topic = "dt/task_manager"
            self.mqtt_client.publish(topic, json.dumps(response))
            rospy.loginfo(f"已发送任务完成响应: {response}")
        except Exception as e:
            rospy.logerr(f"发送任务完成响应失败: {e}")

    def open_door(self, req):
        rospy.loginfo("正在开门...")
        self.move_door_smooth(start_offset=0.0, end_offset=0.75)
        rospy.loginfo("门已打开")
        return EmptyResponse()

    def close_door(self, req):
        rospy.loginfo("正在关门...")
        self.move_door_smooth(start_offset=0.75, end_offset=0.0)
        rospy.loginfo("门已关闭")
        return EmptyResponse()

    def handle_door_control(self, req):
        """处理闸机门控制请求"""
        try:
            rospy.loginfo(f"处理闸机门控制请求: mode={req.mode}")
            mode = req.mode
            
            if mode == 1:  # 开门
                self.left_door_pub.publish(Float64(data=1.5))
                self.right_door_pub.publish(Float64(data=1.5))
                message = "闸机门已打开"
            elif mode == 2:  # 开另一侧门
                self.left_door_pub.publish(Float64(data=-1.5))
                self.right_door_pub.publish(Float64(data=-1.5))
                message = "闸机门已关闭"
            else:
                self.left_door_pub.publish(Float64(data=0))
                self.right_door_pub.publish(Float64(data=0))
                message = f"闸机门控制模式: {mode}"
                
            # 等待命令生效
            rospy.sleep(0.1)
            rospy.loginfo(message)
            
            # 返回响应
            return control_modeResponse(
                success=True,
                message=f"Successfully {req.mode}"
            )
        except Exception as e:
            rospy.logerr(f"闸机门控制失败: {e}")
            return control_modeResponse(
                success=False,
                message=f"控制失败: {e}"
            )

    def shutdown(self):
        """关闭MQTT连接"""
        try:
            self.mqtt_client.loop_stop()
            self.mqtt_client.disconnect()
            rospy.loginfo("MQTT连接已断开")
        except Exception as e:
            rospy.logerr(f"断开MQTT连接时发生错误: {e}")


if __name__ == "__main__":
    rospy.init_node("sliding_door_controller")
    controller = SlidingDoorController()
    
    # 注册关闭回调
    rospy.on_shutdown(controller.shutdown)
    
    rospy.loginfo("电梯和闸机控制器启动完成，等待MQTT和ROS服务调用...")
    rospy.spin()